#import "kGLView.h"
#import "NSFont_OpenGL.h"



@interface kGLView (privatestuff)
- (void)initGL;
- (void) buildFont;
- (void) glPrint:(NSString *)fmt, ...;
- (void) printOverlay;
- (void) updateUI;
@end

@implementation kGLView

- (IBAction)fullScreen:(id)sender
{
	if(FullScreenOn == true)
	{
		[FullScreenWindow close];
		[StartingWindow setContentView: self];
		[StartingWindow makeKeyAndOrderFront: self];
		[StartingWindow makeFirstResponder: self];
		FullScreenOn = false;
	}
	else
	{
		unsigned int windowStyle;
		NSRect	contentRect;
		
		StartingWindow = [NSApp keyWindow];
		windowStyle = NSBorderlessWindowMask;
		contentRect = [[NSScreen mainScreen]frame];
		FullScreenWindow = [[NSWindow alloc]initWithContentRect:contentRect styleMask: windowStyle backing:NSBackingStoreBuffered defer:NO];
		[StartingWindow setAcceptsMouseMovedEvents:NO];
		if(FullScreenWindow != nil)
		{
			NSLog(@"Window was created");
			[FullScreenWindow setTitle: @"myWindow"];
			[FullScreenWindow setReleasedWhenClosed: YES];
			[FullScreenWindow setAcceptsMouseMovedEvents:YES];
			[FullScreenWindow setContentView: self];
			[FullScreenWindow makeKeyAndOrderFront: self];
			[FullScreenWindow setLevel: NSScreenSaverWindowLevel - 1];
            [FullScreenWindow makeFirstResponder:self];
            FullScreenOn = true;
        }
    }
}

- (id)initWithFrame:(NSRect) frameRect
{
    // First, we must create an NSOpenGLPixelFormatAttribute
    NSOpenGLPixelFormat *nsglFormat;
    NSOpenGLPixelFormatAttribute attr[] = 
        {
        NSOpenGLPFADoubleBuffer,
	NSOpenGLPFAAccelerated,
	NSOpenGLPFAColorSize, BITS_PER_PIXEL,
	NSOpenGLPFADepthSize, DEPTH_SIZE,
        0 
        };

    [self setPostsFrameChangedNotifications: YES];

    // Next, we initialize the NSOpenGLPixelFormat itself
    nsglFormat = [[NSOpenGLPixelFormat alloc] initWithAttributes:attr];

    // Check for errors in the creation of the NSOpenGLPixelFormat
    // If we could not create one, return nil (the OpenGL is not initialized, and
    // we should send an error message to the user at this point)
    if(!nsglFormat) { NSLog(@"Invalid format... terminating."); return nil; }

    // Now we create the the CocoaGL instance, using our initial frame and the NSOpenGLPixelFormat
    self = [super initWithFrame:frameRect pixelFormat:nsglFormat];
    [nsglFormat release];
    
    // If there was an error, we again should probably send an error message to the user
    if(!self) { NSLog(@"Self not created... terminating."); return nil; }

    // Now we set this context to the current context (means that its now drawable)
    [[self openGLContext] makeCurrentContext];
	
    // Finally, we call the initGL method (no need to make this method too long or complex)
    [self initGL];
    return self;
}

- (void)initGL
{    	
    // Set the clear color to black. This is the color that your background will be if you don't draw to it
    // If setting to black, we needn't specifically say this, but it's good style to mention explicitly 
    // You can set the clear color to something else later, but it should not be between a glBegin and glEnd
    glClearColor(0.0f, 0.0f, 0.0f, 0.0f);

    // Set the clear depth to 1. This is the initial (default) value.
    // Set the depth function. This specifies when something will be drawn. GL_LESS passes if the incoming
    // depth value is lESS than the current value
    // Turn the depth test on
    glClearDepth(1.0f);
    glDepthFunc(GL_LESS);
    glEnable(GL_DEPTH_TEST);

    // Set up a hint telling the computer to create the nicest (aka "costliest" or "most correct")
    // image it can
    // This hint is for the quality of color and texture mapping
    glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);

    // This hint is for antialiasing
    glHint(GL_POLYGON_SMOOTH_HINT, GL_NICEST);

    first = YES;
	
	Start.x = STARTX;
	Start.y = STARTY;
	Start.z = 0.0;
	
	Goal.x = GOALX;
	Goal.y = GOALY;
	Goal.z = 0.0;
	
	simTalk = [[NSSpeechSynthesizer alloc] init];
	objLock = [[NSLock alloc] init];
	SimWorld = [[world alloc]initWorldWithGoal:Goal];
	
	Cam = [[Camera alloc]initCameraWithView];
	CameraView = 0;
	
	SR2 = [[Rover alloc]initRover];
	[SR2 setStart:&Start];
	[SR2 setGoal:&Goal];
	[SR2 setSenseRadius:0];
	
	worldLoop = 100;
	
	SensorRangePaths = [[NSMutableArray alloc] init];
	[SensorRangePaths addObject:[[[Path alloc]initPathWithRover:SR2
													 Sense:1.0
													 Color:[NSColor blueColor]] autorelease]];
	/*[SensorRangePaths addObject:[[[Path alloc]initPathWithRover:SR2
													 Sense:20.0
													 Color:[NSColor greenColor]] autorelease]];
	[SensorRangePaths addObject:[[[Path alloc]initPathWithRover:SR2
													 Sense:15.0
													 Color:[NSColor cyanColor]] autorelease]];
	[SensorRangePaths addObject:[[[Path alloc]initPathWithRover:SR2
													 Sense:10.0
													 Color:[NSColor magentaColor]] autorelease]];
	[SensorRangePaths addObject:[[[Path alloc]initPathWithRover:SR2
													 Sense:5.0
													 Color:[NSColor yellowColor]] autorelease]];
	[SensorRangePaths addObject:[[[Path alloc]initPathWithRover:SR2
													 Sense:4.0
													 Color:[NSColor orangeColor]] autorelease]];
	[SensorRangePaths addObject:[[[Path alloc]initPathWithRover:SR2
													 Sense:2.5
													 Color:[NSColor purpleColor]] autorelease]];
	[SensorRangePaths addObject:[[[Path alloc]initPathWithRover:SR2
													 Sense:1.0
												     Color:[NSColor brownColor]] autorelease]];
	*/
	[ self buildFont];
}


/*////////////////////////////////////////////////////////////////////////////////////////////
 * Build our bitmap font
 */
- (void) buildFont
{
	NSFont *font;
	
	// 95 since if we do 96, we get the delete character...
	base = glGenLists( 95 );   // Storage for 95 textures (one per character)
							   // font = [ NSFont fontWithName:@"Courier-Bold" size:18 ];
	font = [ NSFont fontWithName:@"Helvetica" size:18 ];
	if( font == nil )
		NSLog( @"font is nil\n" );
	if( ![ font makeGLDisplayListFirst:' ' count:95 base:base ] )
		NSLog( @"Didn't make display list\n" );
}

/*
 * Custom GL "Print" Routine; does this Unicode-style
 */
- (void) glPrint:(NSString *)fmt, ...
{
	NSString *text;
	va_list ap;                        // Pointer To List Of Arguments
	unichar *uniBuffer;
	
	if( fmt == nil || [ fmt length ] == 0 )   // If There's No Text
		return;                                // Do Nothing
	
	va_start( ap, fmt );               // Parses The String For Variables
	text = [ [ [ NSString alloc ] initWithFormat:fmt arguments:ap ] autorelease ];
	va_end( ap );                      // Results Are Stored In Text
	
	glPushAttrib( GL_LIST_BIT );       // Pushes The Display List Bits
	glListBase( base - 32 );           // Sets The Base Character to 32
	uniBuffer = calloc( [ text length ], sizeof( unichar ) );
	[ text getCharacters:uniBuffer ];
	// Draws The Display List Text
	glCallLists( [ text length ], GL_UNSIGNED_SHORT, uniBuffer );
	free( uniBuffer );
	glPopAttrib();                     // Pops The Display List Bits
}

- (void) printOverlay;
{
	glPushAttrib(GL_LIGHTING_BIT | GL_CURRENT_BIT);
	glDisable(GL_LIGHTING);
	
	glPushMatrix();
	glLoadIdentity();
	
	glMatrixMode(GL_PROJECTION);
	glPushMatrix();
	glLoadIdentity();
	gluOrtho2D(0,[self frame].size.width, 0, [self frame].size.height);
	
	glColor4f(YELLOW);
	
	// place data to print in upper left corner here
	// new line is 25 pixels
	////////////////////////////////////////////////////////
	glRasterPos2f(5,[self frame].size.height - 25);
	[self glPrint:@"World - %d",[SimWorld getWorldNum]];
	if(*[SR2 senseRadius] != 0.0){
		glRasterPos2f(5,[self frame].size.height - 50);
		[self glPrint:@"Sensor Radius - %.2f",*[SR2 senseRadius]];
	}
	glRasterPos2f(5,[self frame].size.height - 75);
	[self glPrint:@"Alpha - %3.3f",RADTODEG(*[SR2 getRoverAlpha])];
	glRasterPos2f(5,[self frame].size.height - 100);
	[self glPrint:@"Speed - %d",*[SR2 getRoverSpeed]];
	////////////////////////////////////////////////////////
	glPopMatrix();
	
	glMatrixMode(GL_MODELVIEW);
	glPopMatrix();
	
	glEnable(GL_LIGHTING);
	glPopAttrib();

}

//////////////////////////////////////////////////////////////////////////////////////////////

// awake from nib is called when the window opens
- (void)awakeFromNib
{
	time = [ [NSTimer scheduledTimerWithTimeInterval: DEFAULT_TIME_INTERVAL
                    target:self
                    selector:@selector(drawFrame)	//go to this method whenever the time comes
                    userInfo:nil
                    repeats:YES]
                    retain
                    ];
    // Add our timers to the EventTracking loop
    [[NSRunLoop currentRunLoop] addTimer: time forMode: NSEventTrackingRunLoopMode];
    // Add our timers to the ModelPanel loop
    [[NSRunLoop currentRunLoop] addTimer: time forMode: NSModalPanelRunLoopMode];

	[stepDistField setFloatValue:0.5];
	[breadthField setIntValue:2];
	[angleField setIntValue:45];
	[calcSpinner setDisplayedWhenStopped:NO];
	[showCspaceButton setState:NSOnState];
	[showNumbersButton setState:NSOffState];
	[showDottedPathButton setState:NSOnState];
	[showAllPathwaysButton setState:NSOffState];
	
	NSButtonCell *cell;
	cell = [[[NSButtonCell alloc] init] autorelease];
	[cell setButtonType:NSSwitchButton];
	[cell setTitle:@""];
	
	NSTableColumn *Pdisplay = [pathTableView tableColumnWithIdentifier:@"pathDisplay"];
	[Pdisplay setDataCell:cell];
	[pathColorWell setColor:[NSColor grayColor]];
	[pathColorWell setEnabled:NO];
	[self updateUI];
	
}

// this method is called whenever the window/control is reshaped
// it is also called when the control is first opened
- (void) reshape
{
    float aspect;
    NSSize bound = [self frame].size;
    aspect = bound.width / bound.height;
    // change the size of the viewport to the new width and height
    // this controls the affine transformation of x and y from normalized device 
    // coordinates to window coordinates (from the OpenGl 1.1 reference book, 2nd ed)
    glViewport(0, 0, bound.width, bound.height);
    glMatrixMode(GL_PROJECTION);
    // you must reload the identity before this or you'll lose your picture
    glLoadIdentity();
    gluPerspective(45.0f, (GLfloat)aspect, 0.1f,500.0f);
    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();
}

- (void)drawFrame
{
	static float j=0;
	int i;
    // make this control the First Responder if this is the first time to draw the frame
    // there is probably some notification that would work more efficiently than this silly test
    // but I don't know what it is
		
    if( first )
    {
        first = NO; 
        if ([[NSApp keyWindow] makeFirstResponder:self] ) {
            NSLog( @"self apparently made first responder" );
			// makes the entire screen detect mouse movements
			[[NSApp keyWindow] setAcceptsMouseMovedEvents:YES]; // turns on mouse movement tracking all the time
		}
        else 
            NSLog( @"self is not first responder"); 
    }
 
    // Clear the buffers!
    glLoadIdentity();
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  	//NSLog(@"threadcreated");
    // this is where you would want to draw
	/////////////////////////////////////////////////////////////////////////////////
	// place drawing elements here
	[Cam updateCamera];
	if(CameraView == 1)[Cam setLook:[SR2 getRoverPosition]];
	else if(CameraView == 2)
	{
		[Cam setLook:[SR2 getRoverPosition]];
		[Cam setNormal:*[SR2 getHeading]];
	}

	[SimWorld DrawGrid];
	[SimWorld GlobalGoal];
	
	if([objLock tryLock]){
		[SimWorld DrawWorldObstacles:YES Cspace:([showCspaceButton state] == NSOnState)?YES:NO];
		[objLock unlock];
	}
	
	[SR2 drawRover];
	[SR2 updateRover];
	
	if([showNumbersButton state] == NSOnState){ // prints out the object number next to each object
		glColor4f(WHITE);
		for(i=0; i<[SimWorld getNumberOfCspaceObjs]; i++){
			glRasterPos2f([[[SimWorld getConfigurationArray] objectAtIndex:i]getCenter]->x,
						[[[SimWorld getConfigurationArray] objectAtIndex:i]getCenter]->y);
			[self glPrint:@"%d",[[[SimWorld getConfigurationArray] objectAtIndex:i]getObjectNum]];
		}
	}
	
	if([showDottedPathButton state] == NSOnState){ // draws a dotted line from the start to the goal
		glEnable(GL_LINE_STIPPLE);
		glColor4f(WHITE);
		if(sin(j+=0.08) > 0) glLineStipple(1, 0xf0f0);
		else glLineStipple(1, 0x0f0f);
		glBegin(GL_LINE_STRIP);
		//glVertex3f(Start.x,Start.y,-0.1);
		glVertex3f([SR2 getRoverPosition]->x,[SR2 getRoverPosition]->y,-0.1);
		glVertex3f(Goal.x,Goal.y,-0.1);
		glEnd();
		glDisable(GL_LINE_STIPPLE);
	}
	
	//glTranslatef(0.0f,0.0f,1.0f);
	glColor4f(GREEN);
	glRasterPos2f(Start.x,Start.y);
	[self glPrint:@"Start"];
	
	glColor4f(RED);
	glRasterPos2f(Goal.x,Goal.y);
	[self glPrint:@"Goal"];
	
	for(i=0;i<[SensorRangePaths count];i++){
		[[SensorRangePaths objectAtIndex:i] DrawPathways:[showAllPathwaysButton state]];
	}
	
	[self printOverlay];
    // flush the buffer! (send drawing to the screen)
    [[self openGLContext] flushBuffer];
}

/* This is the preferred way to determine events */
// if you want to get events sent to you, you must make yourself a first responder
- (BOOL)acceptsFirstResponder
{
    return YES;
}
- (BOOL)becomeFirstResponder
{
    return YES;
}
// handle key down events
// if you don't handle this, the system beeps when you press a key (how annoying)
- (void)keyDown:(NSEvent *)event
{
	static int tempR = 0;
	static int i=0;
	
	unichar keyChar = 0;
	if([event modifierFlags] & NSNumericPadKeyMask){
		NSString *theArrow = [event charactersIgnoringModifiers];
		
		if([theArrow length] == 0) return; //reject dead keys
		if([theArrow length] == 1) {
			keyChar = [theArrow characterAtIndex:0];
			
			switch(keyChar){
				case NSUpArrowFunctionKey:
					[SR2 Speed: 1];
				return;
			
				case NSDownArrowFunctionKey:
					[SR2 Speed: -1];
				return;
	
				case NSLeftArrowFunctionKey:
					[SR2 Alpha: 0.01745];
				return;
			
				case NSRightArrowFunctionKey:
					[SR2 Alpha: -0.01745];
				return;
			}
			[super keyDown:event];
		}
	}
	else {
		 NSString *keyStroke = [event charactersIgnoringModifiers];
		if([keyStroke length] == 0) return;
		if([keyStroke length] == 1){
			keyChar = [keyStroke characterAtIndex:0];
			switch(keyChar){
				case ' ':
					//[[SensorRangePaths objectAtIndex:0] cycle];
					[SR2 setSpeed:0.0];
					return;
				case 's':
					return;
				case 'h':
					CameraView++;
					if(CameraView > 3)CameraView = 0;
					if(CameraView == 0)
					{
						[Cam setLookX:0 Y:0 Z:0];
						[Cam setPitch:0.0];
						[Cam setZoom:40.0];
						[Cam setNormalNorth];
					}
					return;
				case 'w':
				{
					Path *path = [SensorRangePaths objectAtIndex:0];
					[path setPathBreadthPerPass:[breadthField intValue]];
					[path setAngularLimit:[angleField intValue]*PI/180];
					[path setPathStep:[stepDistField floatValue]];
					[path setSharedCspaceArray:[SimWorld getConfigurationArray]];
					[SR2 setSenseRadius:[path senseRadius]];
					[path testObj:i];
					i++;

					if(i>=[SimWorld getNumberOfCspaceObjs]) i=0;
				}
					return;
				case 'e':
					[NSThread detachNewThreadSelector:@selector(testThread:) toTarget:self withObject:nil];
					return;
				case 'r':
					tempR ++;
					if(tempR == 0)[SR2 setSenseRadius:0];
					else if(tempR == 1)[SR2 setSenseRadius:20];
					else if(tempR == 2)[SR2 setSenseRadius:10];
					else if(tempR == 3)[SR2 setSenseRadius:5];
					else {
						[SR2 setSenseRadius:2];
						tempR = -1;
					}
					return;
			}
		}
	}  
	[super keyDown:event];
}
/* handle mouse up events (left mouse button)
- (void)mouseUp:(NSEvent *)theEvent
{
    NSPoint pt = [theEvent locationInWindow];
  //  NSLog(@"click up EVENT %f %f", pt.x, pt.y);
}*/
// handle mouse down events (left mouse button)
- (void)mouseDown:(NSEvent *)theEvent
{
	//NSPoint pt = [self convertPoint:[theEvent locationInWindow] fromView:nil]; // actual openGL window coordinates
    NSPoint pt = [theEvent locationInWindow];

	mouseAct = pt;
}
// handle mouse dragging events (left mouse button)
- (void)mouseDragged:(NSEvent *)theEvent
{
	NSPoint pt = [theEvent locationInWindow];
	if([theEvent modifierFlags] & NSControlKeyMask) //hold down the control key and drag the mouse to zoom in or out
		[Cam Zoom: (mouseAct.y - pt.y)*0.025];
	else if([theEvent modifierFlags] & NSShiftKeyMask) // hold down the shift key and drag the mouse to pitch up and down
		[Cam Pitch: -(DEGTORAD(mouseAct.y - pt.y)*.05)];
	else if(CameraView == 0)
	{
		[Cam lookX: (mouseAct.x - pt.x)*[Cam getZoom]*0.001125 // drag the mouse to move the view
				 Y: (mouseAct.y - pt.y)*[Cam getZoom]*0.001125
				 Z: 0];
	}	
	mouseAct = pt;
}
/* handle mouse movement events (left mouse button)
- (void)mouseMoved:(NSEvent *)theEvent
{
    NSPoint pt = [theEvent locationInWindow];
    //NSLog(@"Move EVENT %f %f", pt.x, pt.y);
}
// handle mouse up events (right mouse button)
- (void)rightMouseUp:(NSEvent *)theEvent
{
    NSLog( @"Mouse R up");
}
// handle mouse up events (other mouse button)
- (void)otherMouseUp:(NSEvent *)theEvent
{
    NSLog( @"Mouse O up");
}*/

- (void)CameraView
{
	if(CameraView >= 3) CameraView = 0;
	CameraView++;
}

- (char)getCameraView
{
	return CameraView;
}

///////////////////////////////////////////////////////////////////////////////////////////
// functions for UI and Path table
///////////////////////////////////////////////////////////////////////////////////////////
- (IBAction) deleteAPath:(id)sender
{
	int row = [pathTableView selectedRow];
	if(row != -1){
		[SensorRangePaths removeObjectAtIndex:row];
		[self updateUI];
	}
	else{ NSBeep();}
}

- (IBAction) newPath:(id)sender
{
	[SensorRangePaths addObject:[[[Path alloc]initPathWithRover:SR2
														  Sense:0.0
														  Color:[NSColor redColor]] autorelease]];
	[self updateUI];
}

- (IBAction) setPathColor:(id)sender
{
	int row = [pathTableView selectedRow];
	if(row != -1){
		Path *path = [SensorRangePaths objectAtIndex:row];
		[path setPcolor:[sender color]];
	}
	else{ NSBeep(); }
}

- (IBAction) getWorldLoop:(id)sender
{
	if([sender intValue] < 1) [sender setIntValue:1];
	worldLoop = [sender intValue];
	[SimWorld WorldCountReset];
}

- (IBAction) getObstacleCount:(id)sender
{
	if([sender intValue] < 1) [sender setIntValue:1];
	[SimWorld setNumObstacles:[sender intValue]];
	[SimWorld WorldCountReset];
}

- (IBAction) checkBreadthField:(id)sender
{
	if([sender intValue] < 1) [sender setIntValue:1];
}

- (IBAction) checkAngleField:(id)sender
{
	if([sender intValue] < 1) [sender setIntValue:1];
}

- (IBAction) stepField:(id)sender
{
	[self checkStep];
}

- (void) checkStep
{
	int i;
	float tRange = nil;
	
	for(i=0;i<[SensorRangePaths count];i++){
		Path *path = [SensorRangePaths objectAtIndex:i];
		if([path senseRadius] != 0 && ([path senseRadius] < tRange || tRange == nil)) tRange = [path senseRadius];
	}
	if(tRange == nil) tRange = 0.5;
	if([stepDistField floatValue] > tRange || [stepDistField floatValue] <= 0.0)[stepDistField setFloatValue:tRange];
}

- (void) updateUI
{
	[pathTableView reloadData];
	[self checkStep];
	[deletePath setEnabled: ([SensorRangePaths count] > 1)];
	[worldsField setIntValue:worldLoop];
	[obstacleCountField setIntValue:[SimWorld numObstacles]];
	[toGoalField setFloatValue:magnitude3d([SR2 getRoverPosition],&Goal)];
}

- (int)numberOfRowsInTableView:(NSTableView *)aTableView
{
	return [SensorRangePaths count];
}

// sets the table view with the information needed
- (id)tableView:(NSTableView *)aTableView
objectValueForTableColumn:(NSTableColumn *)aTableColumn
			row:(int)rowIndex
{
	NSString *identifier = [aTableColumn identifier];
	Path *path = [SensorRangePaths objectAtIndex:rowIndex];
	return [path valueForKey:identifier];
}

// if the table view is edited the new values are saved to objects they repersent
- (void)tableView:(NSTableView *)aTableView
   setObjectValue:(id)anObject
   forTableColumn:(NSTableColumn *)aTableColumn
			  row:(int)rowIndex
{
	NSString *identifier = [aTableColumn identifier];
	Path *path = [SensorRangePaths objectAtIndex:rowIndex];
	[path takeValue:anObject forKey:identifier];
}

// updates the cell attributes, colors the background with the color of the path
- (void)tableView:(NSTableView *)aTableView
  willDisplayCell:(id)aCell
   forTableColumn:(NSTableColumn *)aTableColumn
			  row:(int)rowIndex
{
	Path *path = [SensorRangePaths objectAtIndex:rowIndex];
	if([[aTableColumn identifier] isEqual:@"senseRadius"]){
		[aCell setBackgroundColor:[path Pcolor]];
		[aCell setDrawsBackground:YES];
	}
}

- (void)tableViewSelectionDidChange:(NSNotification *)aNote
{
	int row = [pathTableView selectedRow];
	if(row != -1){
		Path *path = [SensorRangePaths objectAtIndex:row];
		[pathColorWell setEnabled:YES];
		[pathColorWell setColor:[path Pcolor]];
	}
	else{
		[pathColorWell setColor:[NSColor grayColor]];
		[pathColorWell setEnabled:NO];
	}
}	

- (IBAction) recalcPaths:(id)sender
{
	[self updateUI];
	[NSThread detachNewThreadSelector:@selector(calcThread:) toTarget:self withObject:nil];	
}

- (IBAction) nextWorld:(id)sender
{
	[self updateUI];
	[SimWorld ResetObstacles];
	[SR2 reset];
	[NSThread detachNewThreadSelector:@selector(calcThread:) toTarget:self withObject:nil];	
}

- (IBAction) startStopButton:(id)sender
{
	[self updateUI];
	if([sender state] == NSOffState) [sender setTitle:@"Run"];
	else{
		[sender setTitle:@"Stop"];
		[NSThread detachNewThreadSelector:@selector(calcLoopThread:) toTarget:self withObject:nil];
	}
}

- (IBAction) stepPathButton:(id)sender
{
	static int i=0;
	static bool start = YES;
	static Point3f tempStart;
	
	[self updateUI];
	if(i < [SensorRangePaths count]){
		if(start){
			tempStart = *[SR2 getRoverPosition];
			start = NO;
		}
	
		Path *path = [SensorRangePaths objectAtIndex:i];
		[path setPathBreadthPerPass:[breadthField intValue]];
		[path setAngularLimit:[angleField intValue]*PI/180];
		[path setPathStep:[stepDistField floatValue]];
		[path setSharedCspaceArray:[SimWorld getConfigurationArray]];
		[SR2 setSenseRadius:[path senseRadius]];
		if([path DrivePath]){
			i++;
			[SR2 setRoverPosition:&tempStart];
			start = YES;
			[simTalk startSpeakingString:@"path complete"];
		}
	}
	else{
		i=0;
		[simTalk startSpeakingString:@"move rover"];
	}
	
	NSLog(@"-------------------------------");
}

- (void) testThread:(id)anObject
{
	NSAutoreleasePool *pool = [[ NSAutoreleasePool alloc] init];
	Path *path = [SensorRangePaths objectAtIndex:0];
	[path setPathBreadthPerPass:[breadthField intValue]];
	[path setAngularLimit:[angleField intValue]*PI/180];
	[path setPathStep:[stepDistField floatValue]];
	[path setSharedCspaceArray:[SimWorld getConfigurationArray]];
	[SR2 setSenseRadius:[path senseRadius]];
	[path testObj:1];
	
	//[[SensorRangePaths objectAtIndex:0] testObj:1];
	[pool release];
	[NSThread exit];
}

- (void) calcThread:(id)anObject
{
	int i;
	Point3f tempStart;
	
	NSAutoreleasePool *pool = [[ NSAutoreleasePool alloc] init];

	[recalcButton setEnabled:NO];
	[nextWorldButton setEnabled:NO];
	[calcSpinner startAnimation:self];
	if([calcPathsButton state] == NSOnState){
		tempStart = *[SR2 getRoverPosition];
		for(i=0;i<[SensorRangePaths count];i++){
			Path *path = [SensorRangePaths objectAtIndex:i];
			[path setPathBreadthPerPass:[breadthField intValue]];
			[path setAngularLimit:[angleField intValue]*PI/180];
			[path setPathStep:[stepDistField floatValue]];
			[path setSharedCspaceArray:[SimWorld getConfigurationArray]];
			[SR2 setRoverPosition:&tempStart];
			while(![path DrivePath]);
		}
	}
	[SR2 setRoverPosition:&tempStart];
	[recalcButton setEnabled:YES];
	[nextWorldButton setEnabled:YES];
	[calcSpinner stopAnimation:self];
	[pathTableView reloadData];
	
	[pool release];
	[NSThread exit];
}

- (void) calcLoopThread:(id)anObject
{
	int i;
	NSSavePanel *SaveP;
	int			fileResult;
	NSString	*header;
	NSString	*s;
	NSDate		*today;
	Path		*path;
	
	NSAutoreleasePool *pool = [[ NSAutoreleasePool alloc] init];
	
	[recalcButton setEnabled:NO];
	[nextWorldButton setEnabled:NO];
	[calcSpinner startAnimation:self];
	
	if([calcPathsButton state] == NSOffState){
		while(worldLoop > 0){
			if([runButton state] == NSOffState) break;
			[objLock lock];
			[SimWorld ResetObstacles];
			[objLock unlock];
			worldLoop--;
		}
	}
	else{
		
///////////////////////////////////////////////////////////////////////
// log file header creation
		SaveP = [NSSavePanel savePanel];
		fileResult = [SaveP runModalForDirectory:@"/Users/mattroman/Documents/robot_stuff/PhD/SSE results/" file:@""];
		NSLog([SaveP filename]);
		
		if(fileResult == NSOKButton){
			logging = YES;
			today = [NSDate date];
			s = @"SSE ";
			header = [s stringByAppendingString:[today descriptionWithCalendarFormat:@"%m-%d-%Y %H:%M:%S" timeZone:nil locale:nil]];
			s = [header stringByAppendingFormat:@"\r\nStart\t%2.1f\t%2.1f\r\nEnd\t%2.1f\t%2.1f\r\nWorld with %d Obstacles\r\nDistance to Goal from Start %1.3f\r\n%d world iterations of %d paths\r\nSearch Tree breadth %d\r\n"
				 ,Start.x,Start.y,Goal.x,Goal.y							
				 ,[SimWorld numObstacles]
				 ,magnitude3d(&Start,&Goal)
				 ,worldLoop
				 ,[SensorRangePaths count]
				 ,[breadthField intValue]];
	
			[@" " writeToFile:[SaveP filename] atomically:YES];
			StatFile = [NSFileHandle fileHandleForWritingAtPath:[SaveP filename]];
			[StatFile retain];
			[StatFile writeData:[s dataUsingEncoding:NSASCIIStringEncoding]];
			[StatFile writeData:[@"\r\nWorld#\tArea(\%)\tCspace" dataUsingEncoding:NSASCIIStringEncoding]];
			for(i=0;i<[SensorRangePaths count];i++){
				path = [SensorRangePaths objectAtIndex:i];
				s = [NSString stringWithFormat:@"\t%2.2f\t\t",[path senseRadius]];
				[StatFile writeData:[s dataUsingEncoding:NSASCIIStringEncoding]];
			}
		}
///////////////////////////////////////////////////////////////////////
		else logging = NO;
		
		[SR2 reset];
		
		while(worldLoop > 0){
			[objLock lock];
			[SimWorld ResetObstacles];
			[objLock unlock];
			
			if(logging){
				s = [NSString stringWithFormat:@"\r\n%d\t%2.3f\t%d\t",[SimWorld getWorldNum],[SimWorld getCoverageArea],[SimWorld getNumberOfCspaceObjs]];
				[StatFile writeData:[s dataUsingEncoding:NSASCIIStringEncoding]];
			}
			
			for(i=0;i<[SensorRangePaths count];i++){
				path = [SensorRangePaths objectAtIndex:i];
				[path setPathBreadthPerPass:[breadthField intValue]];
				[path setAngularLimit:[angleField intValue]*PI/180];
				[path setPathStep:[stepDistField floatValue]];
				[path setSharedCspaceArray:[SimWorld getConfigurationArray]];
				while(![path DrivePath]);
				[SR2 reset];
				if(logging){
					s = [NSString stringWithFormat:@"%2.4f\t",[path pathDistance]];
					[StatFile writeData:[s dataUsingEncoding:NSASCIIStringEncoding]];
					if([path incompletePath]){
						s = [NSString stringWithFormat:@"NAP\t\t"];
						[StatFile writeData:[s dataUsingEncoding:NSASCIIStringEncoding]];
					}
					else{
						s = [NSString stringWithFormat:@"%2.4f\t",[path pathEfficiency]];
						[StatFile writeData:[s dataUsingEncoding:NSASCIIStringEncoding]];
						s = [NSString stringWithFormat:@"%d\t",[path roboTurn]];
						[StatFile writeData:[s dataUsingEncoding:NSASCIIStringEncoding]];
					}
				}
			}
			worldLoop--;
			if([runButton state] == NSOffState) break;
			[self updateUI];
		}
		
		if(logging){
			[StatFile closeFile];
			[StatFile release];
		}
	}
 
	[recalcButton setEnabled:YES];
	[nextWorldButton setEnabled:YES];
	[calcSpinner stopAnimation:self];
	[pathTableView reloadData];
	[runButton setState:NSOffState];
	[runButton setTitle:@"Run"];
	[SR2 reset];
	[simTalk startSpeakingString:@"Run complete."];
	
	[pool release];
	[NSThread exit];
}

- (void)windowWillClose:(NSNotification *)notification
{
	NSLog(@"All Done, Good Bye");
	[NSApp terminate:nil];
}
@end
